/*!
  @file sliding_window.cpp
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#include "openlab2013/sliding_window.h"

#include <iostream>
#include <fstream>

#include <ros/ros.h>
#include <Eigen/Geometry>

#include "sba/eucsbademo.h"

namespace mrover
{

void SlidingWindow::performSBA()
{
  const size_t si = poses_.size() - w_size_;
  const size_t ei = poses_.size() - 1;


  ///////////////////////////
  // save to txt files..
  ///////////////////////////
  char file_cams[128];
  sprintf(file_cams, "%s/cams.txt", work_dir_.c_str());
  std::ofstream ofs_cams(file_cams);
  if (!ofs_cams.is_open())
  {
    ROS_ERROR("File open error during SBA");
    return;
  }

  Eigen::Affine3f pose;
  cv::Mat mTs = poses_.at(si).inv();
  for (size_t ncam = si; ncam < si + w_size_; ++ncam)
  {
    cv::Mat cTs = poses_.at(ncam) * mTs;
    for (int i = 0; i < 3; ++i)
    {
      for (int j = 0; j < 3; ++j)
        pose.linear()(i, j) = cTs.at<float>(i, j);
      pose.translation()(i) = cTs.at<float>(i, 3);
    }
    Eigen::Quaternionf q(pose.linear());
    ofs_cams << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " "
             << pose.translation()(0) << " " << pose.translation()(1) << " " << pose.translation()(2)
             << std::endl;
  }

  ofs_cams.close();



  char file_pts[128];
  sprintf(file_pts, "%s/pts.txt", work_dir_.c_str());
  std::ofstream ofs_pts(file_pts);
  for (size_t ncam = si; ncam < si + w_size_ - 1; ++ncam)
  {
    CloudPtr cloud(new Cloud);
    cv::Mat _sTn = poses_[si] * poses_[ncam].inv();
    Eigen::Matrix4f sTn;
    for (int i = 0; i < 4; ++i)
      for (int j = 0; j < 4; ++j)
        sTn(i, j) = _sTn.at<float>(i, j);
    pcl::transformPointCloud(*points_[ncam], *cloud, sTn); // transform cloud
    for (size_t i = 0; i < cloud->points.size(); ++i)
    {
      ofs_pts 
          << cloud->points[i].x << " " << cloud->points[i].y << " " << cloud->points[i].z << " "
          << 2 << " "
          << ncam-si   << " " << proj0_[ncam]->at(i).pt.x << " " << proj0_[ncam]->at(i).pt.y << " "
          << ncam-si+1 << " " << proj1_[ncam]->at(i).pt.x << " " << proj1_[ncam]->at(i).pt.y
          << std::endl;
    }
  }
  ofs_pts.close();


  char file_calib[128];
  sprintf(file_calib, "%s/calib.txt", work_dir_.c_str());
  static bool init = false;
  if (!init) 
  {
    std::ofstream ofs_calib(file_calib);
    for (int i = 0; i < 3; ++i)
    {
      for (int j = 0; j < 3; ++j)
        ofs_calib << K_.at<float>(i, j) << " ";
      ofs_calib << std::endl;
    }
    ofs_calib.close();
    init = true;
  }


  ///////////////////////////
  // sba
  ///////////////////////////
  int cnp = 6;
  double *motstruct;
  double *filtered = (double *)malloc((cnp+1)*sizeof(double));
  char* mc = const_cast<char*>("-");
  sba_driver(file_cams, file_pts, file_calib, cnp, 3, 2, quat2vec, vec2quat, cnp+1, mc, mc, &motstruct);
  cv::Mat ps;
  for (int ncam = si; ncam < si + w_size_; ++ncam) 
  {
    //FILE* fp = stdout;
    //fprintf(fp, "%.10e ", motstruct[i]);
    //if((i+1)%cnp==0) fputc('\n', fp);

    vec2quat(motstruct, cnp, filtered, cnp+1);
    Eigen::Quaternionf q(filtered[0], filtered[1], filtered[2], filtered[3]);
    Eigen::Matrix3f m = q.toRotationMatrix();

    cv::Mat p = cv::Mat::eye(4, 4, CV_32F);
    for (int i = 0; i < 3; ++i)
    {
      for (int j = 0; j < 3; ++j)
        p.at<float>(i, j) = m(i, j);
      p.at<float>(i, 3) = filtered[4 + i];
    }

    if (ncam == si) 
    {
      ps = p * poses_[si];
    }
    else
    {
      poses_[ncam] = p * poses_[ncam] * ps.inv() * poses_[si];
    }

    //.clone();poses_[ncam] = p * poses_[ncam] * p0_inv;
    //poses_[ncam] = poses_[si];
  }
  free(motstruct);
}
#if 0
void SlidingWindow::performSBA()
{
  ///////////////////////////
  // save to txt files..
  ///////////////////////////
  char file_cams[128];
  sprintf(file_cams, "%s/cams.txt", work_dir_.c_str());
  std::ofstream ofs_cams(file_cams);
  if (!ofs_cams.is_open())
  {
    ROS_ERROR("File open error during SBA");
    return;
  }
  ofs_cams << "1 0 0 0 0 0 0\n";

  Eigen::Affine3f pose;
  cv::Mat cT0 = cv::Mat::eye(4, 4, CV_32F);
  for (size_t ncam = 0; ncam < poses_.size(); ++ncam)
  {
    cT0 = poses_[ncam] * cT0;
    for (int i = 0; i < 3; ++i)
    {
      for (int j = 0; j < 3; ++j)
        pose.linear()(i, j) = cT0.at<float>(i, j);
      pose.translation()(i) = cT0.at<float>(i, 3);
    }
    Eigen::Quaternionf q(pose.linear());
    ofs_cams << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " "
             << pose.translation()(0) << " " << pose.translation()(1) << " " << pose.translation()(2)
             << std::endl;
  }

  ofs_cams.close();



  char file_pts[128];
  sprintf(file_pts, "%s/pts.txt", work_dir_.c_str());
  std::ofstream ofs_pts(file_pts);
  for (size_t ncam = 0; ncam < points_.size(); ++ncam)
  {
    for (size_t i = 0; i < points_[ncam]->points.size(); ++i)
    {
      ofs_pts << points_[ncam]->points[i].x << " "
              << points_[ncam]->points[i].y << " "
              << points_[ncam]->points[i].z << " "
              << 2 << " "
              << ncam   << " " << proj0_[ncam][i].pt.x << " " << proj0_[ncam][i].pt.y << " "
              << ncam+1 << " " << proj1_[ncam][i].pt.x << " " << proj1_[ncam][i].pt.y
              << std::endl;
    }
  }
  ofs_pts.close();


  char file_calib[128];
  sprintf(file_calib, "%s/calib.txt", work_dir_.c_str());
  static bool init = false;
  if (!init) 
  {
    std::ofstream ofs_calib(file_calib);
    for (int i = 0; i < 3; ++i)
    {
      for (int j = 0; j < 3; ++j)
        ofs_calib << K_.at<float>(i, j) << " ";
      ofs_calib << std::endl;
    }
    ofs_calib.close();
    init = true;
  }


  ///////////////////////////
  // sba
  ///////////////////////////
  int cnp = 6;
  double *motstruct;
  double *filtered = (double *)malloc((cnp+1)*sizeof(double));
  char* mc = const_cast<char*>("-");
  sba_driver(file_cams, file_pts, file_calib, cnp, 3, 2, quat2vec, vec2quat, cnp+1, mc, mc, &motstruct);
  rect_poses_.clear();
  cv::Mat p0;
  for (int ncam = 0; ncam < poses_.size(); ++ncam) 
  {
    //FILE* fp = stdout;
    //fprintf(fp, "%.10e ", motstruct[i]);
    //if((i+1)%cnp==0) fputc('\n', fp);

    vec2quat(motstruct, cnp, filtered, cnp+1);
    Eigen::Quaternionf q(filtered[0], filtered[1], filtered[2], filtered[3]);
    Eigen::Matrix3f m = q.toRotationMatrix();

    cv::Mat p = cv::Mat::eye(4, 4, CV_32F);
    for (int i = 0; i < 3; ++i)
    {
      for (int j = 0; j < 3; ++j)
        p.at<float>(i, j) = m(i, j);
      p.at<float>(i, 3) = filtered[4 + i];
    }
    if (ncam == 0) p0 = p.clone();

    //ROS_INFO_STREAM("p" << std::endl << p);
    rect_poses_.push_back(p0.inv() * p * poses_[ncam]);
  }
  free(motstruct);
}
#endif

}  // namespace mrover
